package com.robotic.tcp;

import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.stereotype.Component;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.net.Socket;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.charset.StandardCharsets;
import java.time.LocalDateTime;
import java.time.format.DateTimeFormatter;
import java.util.concurrent.atomic.AtomicInteger;

@Slf4j
@Component
public class RobotNavigator {

    @Value("${robotic.robotIp}")
    private String robotIp;

    @Value("${robotic.robotTcpPort}")
    private int robotPort;

    // 【关键改动1】: 创建一个独立的、用于导航命令的报文ID生成器
    private static final AtomicInteger navigationMessageId = new AtomicInteger(0);

    // 【关键改动2】: 移除对 RobotMonitor 的注入
    // @Autowired
    // private RobotMonitor robotMonitor;

    /**
     * 向机器人发送导航指令，并阻塞等待直到任务完成。
     * 此方法会创建并管理自己的网络连接。
     *
     * @param params 导航任务参数。
     * @return ErrorCode: 0=成功, 其他为失败, -1=通信错误。
     */
    public int sendNavigationCommandAndWait(NavigationParameters params) {
        int currentMessageId = navigationMessageId.getAndIncrement();

        // 【关键改动3】: 使用 try-with-resources 创建独立的、临时的Socket连接
        try (Socket socket = new Socket()) {
            // 【关键改动4】: 为导航任务设置独立的连接和超时策略
            socket.connect(new java.net.InetSocketAddress(robotIp, robotPort), 5000); // 5秒连接超时
            socket.setSoTimeout(0); // 0 = 无限读取超时，以等待机器人完成物理移动

            log.info("RobotNavigator 为导航任务 (ID: {}) 建立独立连接成功。", currentMessageId);

            DataOutputStream out = new DataOutputStream(socket.getOutputStream());
            DataInputStream in = new DataInputStream(socket.getInputStream());

            String asduXml = buildNavigationXml(params);
            byte[] asduBytes = asduXml.getBytes(StandardCharsets.UTF_8);
            byte[] header = buildHeader(asduBytes.length, currentMessageId);

            log.info("正在发送导航指令... 报文ID: {}, ASDU 长度: {}", currentMessageId, asduBytes.length);
            out.write(header);
            out.write(asduBytes);
            out.flush();
            log.info("导航指令已发送。正在无限时等待机器人响应...");

            // --- 读取响应 ---
            byte[] responseHeader = new byte[16];
            in.readFully(responseHeader);

            ByteBuffer headerBuffer = ByteBuffer.wrap(responseHeader).order(ByteOrder.LITTLE_ENDIAN);
            if (headerBuffer.getInt() != 0x90eb90eb || (headerBuffer.getShort(6) & 0xFFFF) != currentMessageId) {
                log.error("导航任务(ID: {})失败：收到无效或ID不匹配的响应头。", currentMessageId);
                return -1;
            }

            int responseAsduLength = headerBuffer.getShort(4) & 0xFFFF;
            byte[] responseAsduBytes = new byte[responseAsduLength];
            in.readFully(responseAsduBytes);
            String responseXml = new String(responseAsduBytes, StandardCharsets.UTF_8);
            log.info("收到导航任务(ID: {})的响应XML: {}", currentMessageId, responseXml);

            String errorCodeStr = parseTagValue(responseXml, "ErrorCode");
            return Integer.parseInt(errorCodeStr);

        } catch (IOException e) {
            log.error("导航任务 (ID: {}) 与机器人通信时出错: {}", currentMessageId, e.getMessage());
            return -1;
        } catch (Exception e) {
            log.error("导航任务 (ID: {}) 发生未知错误: {}", currentMessageId, e.getMessage());
            return -1;
        }
    }

    /**
     * 向机器人发送取消导航任务的指令。
     * @return 操作成功则返回true，否则返回false。
     */
    public boolean cancelNavigation() {
        int currentMessageId = navigationMessageId.getAndIncrement();
        String asduXml = "<?xml version=\"1.0\" encoding=\"UTF-8\"?><PatrolDevice><Type>1004</Type><Command>1</Command><Time>"
                + DateTimeFormatter.ofPattern("yyyy-MM-dd HH:mm:ss").format(LocalDateTime.now())
                + "</Time><Items/></PatrolDevice>";

        try (Socket socket = new Socket()) {
            // 取消是快速操作，使用较短的超时
            socket.connect(new java.net.InetSocketAddress(robotIp, robotPort), 2000); // 2秒连接超时
            socket.setSoTimeout(5000); // 5秒读取超时

            DataOutputStream out = new DataOutputStream(socket.getOutputStream());
            DataInputStream in = new DataInputStream(socket.getInputStream());

            byte[] asduBytes = asduXml.getBytes(StandardCharsets.UTF_8);
            byte[] header = buildHeader(asduBytes.length, currentMessageId);

            out.write(header);
            out.write(asduBytes);
            out.flush();

            byte[] responseHeader = new byte[16];
            in.readFully(responseHeader);

            ByteBuffer headerBuffer = ByteBuffer.wrap(responseHeader).order(ByteOrder.LITTLE_ENDIAN);
            if (headerBuffer.getInt() != 0x90eb90eb || (headerBuffer.getShort(6) & 0xFFFF) != currentMessageId) {
                log.error("取消失败：收到无效或ID不匹配的响应。");
                return false;
            }

            int responseAsduLength = headerBuffer.getShort(4) & 0xFFFF;
            byte[] responseAsduBytes = new byte[responseAsduLength];
            in.readFully(responseAsduBytes);
            String responseXml = new String(responseAsduBytes, StandardCharsets.UTF_8);

            String errorCodeStr = parseTagValue(responseXml, "ErrorCode");
            log.info("收到取消指令的响应XML: {}", responseXml);

            return "0".equals(errorCodeStr);

        } catch (IOException e) {
            log.error("发送取消指令 (ID: {}) 时出错: {}", currentMessageId, e.getMessage());
            return false;
        }
    }

    // --- 辅助方法 (保持不变) ---
    private byte[] buildHeader(int asduLength, int msgId) { /* ... 内容不变 ... */
        ByteBuffer buffer = ByteBuffer.allocate(16);
        buffer.order(ByteOrder.LITTLE_ENDIAN);
        buffer.putInt(0x90eb90eb);
        buffer.putShort((short) asduLength);
        buffer.putShort((short) msgId);
        buffer.put(new byte[8]);
        return buffer.array();
    }
    private String buildNavigationXml(NavigationParameters params) { /* ... 内容不变 ... */
        DateTimeFormatter dtf = DateTimeFormatter.ofPattern("yyyy-MM-dd HH:mm:ss");
        String now = dtf.format(LocalDateTime.now());
        return "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" +
                "<PatrolDevice>" +
                "<Type>1003</Type>" +
                "<Command>1</Command>" +
                "<Time>" + now + "</Time>" +
                "<Items>" +
                "<Value>" + params.getValue() + "</Value>" +
                "<MapID>" + params.getMapID() + "</MapID>" +
                "<PosX>" + params.getPosX() + "</PosX>" +
                "<PosY>" + params.getPosY() + "</PosY>" +
                "<PosZ>" + params.getPosZ() + "</PosZ>" +
                "<AngleYaw>" + params.getAngleYaw() + "</AngleYaw>" +
                "<PointInfo>" + params.getPointInfo() + "</PointInfo>" +
                "<Gait>" + params.getGait() + "</Gait>" +
                "<Speed>" + params.getSpeed() + "</Speed>" +
                "<Manner>" + params.getManner() + "</Manner>" +
                "<ObsMode>" + params.getObsMode() + "</ObsMode>" +
                "<NavMode>" + params.getNavMode() + "</NavMode>" +
                "<Terrain>" + params.getTerrain() + "</Terrain>" +
                "<Posture>" + params.getPosture() + "</Posture>" +
                "</Items>" +
                "</PatrolDevice>";
    }
    private String parseTagValue(String xml, String tagName) { /* ... 内容不变 ... */
        String startTag = "<" + tagName + ">";
        String endTag = "</" + tagName + ">";
        int startIndex = xml.indexOf(startTag);
        if (startIndex == -1) return null;
        int endIndex = xml.indexOf(endTag, startIndex);
        if (endIndex == -1) return null;
        return xml.substring(startIndex + startTag.length(), endIndex);
    }
}
